/*
 * @Author: your name
 * @Date: 2021-06-12 07:28:47
 * @LastEditTime: 2021-06-12 10:01:24
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: /lab_1_2_geometry_unfinished/attitude.cpp
 */
#include "attitude.h"
#include "eigen3/Eigen/Geometry"


#include "math.h"
Eigen::Quaterniond Attitude::toQuaternion() const
{
  // Todo: Implement this correctly using Eigen::AngleAxisd (Z->Y->X).
  Eigen::Matrix3d Rz;
  Eigen::Matrix3d Ry;
  Eigen::Matrix3d Rx;
  Rz<<cos(z_rot),-sin(z_rot),0,sin(z_rot),cos(z_rot),0,0,0,1;
  Ry<<cos(y_rot),0,sin(y_rot),0,1,0,-sin(y_rot),0,cos(y_rot);
  Rx<<1,0,0,0,cos(x_rot),-sin(x_rot),0,sin(x_rot),cos(x_rot);
  // return Eigen::Quaterniond(Eigen::AngleAxisd(length, Eigen::Vector3d(z_rot/length,y_rot/length,x_rot/length)));
  return Eigen::Quaterniond(Rz*Ry*Rx);
}

Sophus::SO3d Attitude::toSO3() const
{
  return Sophus::SO3d(toQuaternion());
}

std::istream& operator>>(std::istream& is, Attitude& att)
{
  is >> att.x_rot
     >> att.y_rot
     >> att.z_rot;

  if (!is)
  {
    throw std::runtime_error("Could not read Attitude data");
  }

  return is;
}

std::ostream& operator<<(std::ostream& os, const Attitude& att)
{
  os << "x_rot: " << att.x_rot << "\n"
     << "y_rot: " << att.y_rot << "\n"
     << "z_rot: " << att.z_rot << "\n";

  return os;
}
